#include "basic.h"

CVec3 O31 = {0,0,0}; 
enum NAV_PROCESS_ENUM NAV_PROCESS = Init_Alignment;

const glv_s glv = 
{
    .Re = 6378137.0,
    .G0 = 9.80665,  
    .r2d = 57.295779513082323,       // rad --> deg
    .d2r = 0.017453292519943,         // deg --> rad        

    .ug = 9.780326771400000e-06,     // ug --> m/ss
    .ugpshz = 9.780326771400000e-06, // ug/sqrt(Hz) --> m/s/sqrt(s)
    .ugpsh = 1.630054461900000e-07,  // ug/sqrt(H) --> m/ss/sqrt(s)

    .dps = 0.017453292519943,        // deg/s --> rad/h
    .dph = 4.848136811095360e-6,     // deg/h --> rad/s
    .dpsh = 2.908882086657216e-4,    // deg/sqrt(h) --> rad/sqrt(s)
    .dphpsh = 8.080228018492267e-08  // deg/h/sqrt(h) --> rad/s/sqrt(s)
};

const imuParam_s imuParam =
{
    .gyrBiasRepeatability = 30.0f * 1.0f,                            // deg/h
    .accBias = {2000.0f,2000.0f,2000.0f},                            // ug
    .ARW     = {0.150f * 1.0f, 0.150f * 1.0f, 0.150f * 1.0f},        // deg/sqrt(h)
    .VRW     = {100.0f * 1.0f, 100.0f * 1.0f, 100.0f * 1.0f},        // ug/sqrt(Hz)
    .sqrtR0G = 0.0f,                                                 // deg/h
    .tauG    = 1000.0f,                                              // s
    .sqrtR0A = 0.0f,                                                 // ug
    .tauA    = 1000.0f
};

const obsParam_s obsParam =
{
    .um982_lever = {0, 0, 0},
    .um982_noise = 
    {
        .yaw = 2.0,
        .spp_vel = {0.2, 0.2, 0.2},
        .spp_pos = {1.5, 1.5, 10.0},
        .rtk_vel = {0.01, 0.01, 0.01},
        .rtk_pos = {0.05, 0.05, 0.1}
    },

    .baroHgt_noise = 1.5,
    .magYaw_noise = 5.0
};

namespace math
{
    const double PI = 3.141592653589793;
    const double MAX_LAT = 1.570796326794897;
    const double MAX_LON = 3.141592653589793;
    const double MAX_HGT = 5000;

    CVec3 q2rv(const CQuat &q)
    {
        CQuat dq = q;
        if (dq.w() < 0)
        {
            dq.w() = -dq.w(), dq.x() = -dq.x(), dq.y() = -dq.y(), dq.z() = -dq.z();
        }
        if (dq.w() > 1.0)
        {
            dq.w() = 1.0;
        }

        double n2 = acos(dq.w());
        double f = 2.0;
        if (n2 > 1.0e-20)
        {
            f = 2.0 / (sin(n2) / n2);
        }
        return CVec3(dq.x() * f, dq.y() * f, dq.z() * f);
    }

    CQuat rv2q(const CVec3 &v)
    {
        double n2 = v.squaredNorm(), c, f;
        if (n2 < (PI / 180.0 * PI / 180.0)) // 0.017^2
        {
            double n4 = n2 * n2;
            c = 1.0 - n2 * (1.0 / 8.0) + n4 * (1.0 / 384.0);
            f = 0.5 - n2 * (1.0 / 48.0) + n4 * (1.0 / 3840.0);
        }
        else
        {
            double n_2 = sqrt(n2) * 0.5;
            c = cos(n_2);
            f = sin(n_2) / n_2 * 0.5;
        }
        return CQuat(c, f * v(0), f * v(1), f * v(2));
    }

    CVec3 q2att(const CQuat &qnb)
    {
        double q11 = qnb.w() * qnb.w(), q12 = qnb.w() * qnb.x(), q13 = qnb.w() * qnb.y(), q14 = qnb.w() * qnb.z(),
               q22 = qnb.x() * qnb.x(), q23 = qnb.x() * qnb.y(), q24 = qnb.x() * qnb.z(),
               q33 = qnb.y() * qnb.y(), q34 = qnb.y() * qnb.z(),
               q44 = qnb.z() * qnb.z();
        double pit = asin(2 * (q34 + q12));
        double rol = atan2(-2 * (q24 - q13), q11 - q22 - q33 + q44);
        double yaw = atan2(-2 * (q23 - q14), q11 - q22 + q33 - q44);
        return CVec3(pit, rol, yaw);
    }

    CVec3 m2att(const CMat3 &Cnb)
    {
        CVec3 att;
        att(0) = asin(Cnb(2,1));
        att(1) = atan2(-Cnb(2,0), Cnb(2,2));
        att(2) = atan2(-Cnb(0,1), Cnb(1,1));

        if (Cnb(2,1) > 0.999999)
        {
            att(1) = 0.0;
            att(2) = atan2(Cnb(0,2), Cnb(0,0));
        }
        else if (Cnb(2,1) < -0.999999)
        {
            att(1) = 0.0;
            att(2) = -atan2(Cnb(0,2), Cnb(0,0));
        }
        return att;
    }

    CMat3 q2mat(const CQuat &qnb)
    {
        double	q11 = qnb.w()*qnb.w(), q12 = qnb.w()*qnb.x(), q13 = qnb.w()*qnb.y(), q14 = qnb.w()*qnb.z(), 
                q22 = qnb.x()*qnb.x(), q23 = qnb.x()*qnb.y(), q24 = qnb.x()*qnb.z(),     
                q33 = qnb.y()*qnb.y(), q34 = qnb.y()*qnb.z(),  
                q44 = qnb.z()*qnb.z();
        CMat3 Cnb;
        Cnb(0,0) = q11+q22-q33-q44,  Cnb(0,1) = 2*(q23-q14),     Cnb(0,2) = 2*(q24+q13),
        Cnb(1,0) = 2*(q23+q14),      Cnb(1,1) = q11-q22+q33-q44, Cnb(1,2) = 2*(q34-q12),
        Cnb(2,0) = 2*(q24-q13),      Cnb(2,1) = 2*(q34+q12),     Cnb(2,2) = q11-q22-q33+q44;
        return Cnb;
    }

    CMat3 a2mat(const CVec3 &att)
    {
        double si = sin(att(0)), ci = cos(att(0)),
               sj = sin(att(1)), cj = cos(att(1)),
               sk = sin(att(2)), ck = cos(att(2));
        CMat3 Cnb;
        Cnb(0,0) = cj * ck - si * sj * sk, Cnb(0,1) = -ci * sk, Cnb(0,2) = sj * ck + si * cj * sk;
        Cnb(1,0) = cj * sk + si * sj * ck, Cnb(1,1) = ci * ck,  Cnb(1,2) = sj * sk - si * cj * ck;
        Cnb(2,0) = -ci * sj,               Cnb(2,1) = si,       Cnb(2,2) = ci * cj;
        return Cnb;
    }

    CQuat a2qua(const CVec3 &att)
    {
        double pitch = att(0) * 0.5, roll = att(1) * 0.5, yaw = att(2) * 0.5;
        double sp = sin(pitch), sr = sin(roll), sy = sin(yaw),
               cp = cos(pitch), cr = cos(roll), cy = cos(yaw);
        CQuat qnb;
        qnb.w() = cp * cr * cy - sp * sr * sy;
        qnb.x() = sp * cr * cy - cp * sr * sy;
        qnb.y() = cp * sr * cy + sp * cr * sy;
        qnb.z() = cp * cr * sy + sp * sr * cy;
        return qnb;
    }

    CQuat m2qua(const CMat3 &Cnb)
    {
        double w, x, y, z, qq4;
        if (Cnb(0,0) >= Cnb(1,1) + Cnb(2,2))
        {
            x = 0.5 * sqrt(1 + Cnb(0,0) - Cnb(1,1) - Cnb(2,2));
            qq4 = 4 * x;
            w = (Cnb(2,1) - Cnb(1,2)) / qq4;
            y = (Cnb(0,1) + Cnb(1,0)) / qq4;
            z = (Cnb(0,2) + Cnb(2,0)) / qq4;
        }
        else if (Cnb(1, 1) >= Cnb(0, 0) + Cnb(2, 2))
        {
            y = 0.5 * sqrt(1 - Cnb(0, 0) + Cnb(1, 1) - Cnb(2, 2));
            qq4 = 4 * y;
            w = (Cnb(0,2) - Cnb(2,0)) / qq4;
            x = (Cnb(0,1) + Cnb(1,0)) / qq4;
            z = (Cnb(1,2) + Cnb(2,1)) / qq4;
        }
        else if (Cnb(2,2) >= Cnb(0,0) + Cnb(1,1))
        {
            z = 0.5 * sqrt(1 - Cnb(0, 0) - Cnb(1,1) + Cnb(2,2));
            qq4 = 4 * z;
            w = (Cnb(1,0) - Cnb(0,1)) / qq4;
            x = (Cnb(0,2) + Cnb(2,0)) / qq4;
            y = (Cnb(1,2) + Cnb(2,1)) / qq4;
        }
        else
        {
            w = 0.5 * sqrt(1 + Cnb(0,0) + Cnb(1,1) + Cnb(2,2));
            qq4 = 4 * w;
            x = (Cnb(2,1) - Cnb(1,2)) / qq4;
            y = (Cnb(0,2) - Cnb(2,0)) / qq4;
            z = (Cnb(1,0) - Cnb(0,1)) / qq4;
        }
        double nq = sqrt(w * w + x * x + y * y + z * z);
        w /= nq, x /= nq, y /= nq, z /= nq;
        return CQuat(w, x, y, z);
    }

    CVec3 qq2phi(const CQuat &qCalc, const CQuat &qReal)
    {
        return q2rv(qReal * qCalc.conjugate());
    }

    CQuat qdelphi(const CQuat &q, const CVec3 &phi)
    {
        CQuat qtmp = rv2q(phi);
        double w = qtmp.w() * q.w() - qtmp.x() * q.x() - qtmp.y() * q.y() - qtmp.z() * q.z();
        double x = qtmp.w() * q.x() + qtmp.x() * q.w() + qtmp.y() * q.z() - qtmp.z() * q.y();
        double y = qtmp.w() * q.y() + qtmp.y() * q.w() + qtmp.z() * q.x() - qtmp.x() * q.z();
        double z = qtmp.w() * q.z() + qtmp.z() * q.w() + qtmp.x() * q.y() - qtmp.y() * q.x();
        return CQuat(w, x, y, z);
    }
}
